{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "49066e96-39d5-490a-971c-9dfa6ac819e5",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "\n",
      "\n",
      "front 78.98306846618652\n",
      "left：71.92254066467285   39.92724418640137   \n",
      "right：14.202117919921875   42.383432388305664   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 62.07752227783203\n",
      "left：41.40663146972656   52.39057540893555   \n",
      "right：12.783527374267578   157.82833099365234   \n",
      "\n",
      "\n",
      "right     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 12.479543685913086\n",
      "left：12.998342514038086   42.65904426574707   \n",
      "right：152.45795249938965   89.17665481567383   \n",
      "\n",
      "\n",
      "right     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 148.2224464416504\n",
      "left：11.644601821899414   12.730836868286133   \n",
      "right：33.43820571899414   127.7461051940918   \n",
      "\n",
      "\n",
      "right     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 151.4730453491211\n",
      "left：121.57726287841797   16.338109970092773   \n",
      "right：134.3851089477539   88.04583549499512   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 131.73842430114746\n",
      "left：65.52672386169434   44.80314254760742   \n",
      "right：97.34773635864258   85.49642562866211   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 159.16991233825684\n",
      "left：12.098550796508789   8.122444152832031   \n",
      "right：121.92177772521973   64.76879119873047   \n",
      "\n",
      "\n",
      "right     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 111.97137832641602\n",
      "left：106.75907135009766   88.53626251220703   \n",
      "right：110.04209518432617   63.228607177734375   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 91.74227714538574\n",
      "left：82.79705047607422   1014.6691799163818   \n",
      "right：60.47654151916504   132.4923038482666   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 31.666994094848633\n",
      "left：15.726089477539062   823.1351375579834   \n",
      "right：88.48762512207031   36.473989486694336   \n",
      "\n",
      "\n",
      "down     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 14.076471328735352\n",
      "left：14.65606689453125   436.88130378723145   \n",
      "right：163.0082130432129   41.179656982421875   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 130.16986846923828\n",
      "left：285.2137088775635   281.663179397583   \n",
      "right：14.246702194213867   62.38555908203125   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 281.04710578918457\n",
      "left：250.16236305236816   117.94161796569824   \n",
      "right：8.681774139404297   18.709182739257812   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 1198.6522674560547\n",
      "left：67.75593757629395   66.56837463378906   \n",
      "right：277.9829502105713   28.18131446838379   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 1197.8659629821777\n",
      "left：56.24103546142578   49.72362518310547   \n",
      "right：1197.6308822631836   117.39850044250488   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 273.62990379333496\n",
      "left：299.93462562561035   1197.7605819702148   \n",
      "right：109.21931266784668   162.83798217773438   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 42.841434478759766\n",
      "left：87.43786811828613   232.0854663848877   \n",
      "right：360.57329177856445   155.5464267730713   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 4.78672981262207\n",
      "left：1197.6349353790283   82.01885223388672   \n",
      "right：3.133058547973633   138.5476589202881   \n",
      "\n",
      "\n",
      "down     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 5.840539932250977\n",
      "left：83.72116088867188   83.85896682739258   \n",
      "right：1197.6308822631836   62.142372131347656   \n",
      "\n",
      "\n",
      "right     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 1198.559045791626\n",
      "left：24.999618530273438   84.12647247314453   \n",
      "right：1197.5376605987549   137.67623901367188   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 1198.068618774414\n",
      "left：227.66757011413574   85.7841968536377   \n",
      "right：1198.011875152588   133.6231231689453   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 22.705554962158203\n",
      "left：20.62225341796875   83.66847038269043   \n",
      "right：1197.772741317749   132.68280029296875   \n",
      "\n",
      "\n",
      "down     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 1200.9828090667725\n",
      "left：45.63403129577637   89.30230140686035   \n",
      "right：61.56277656555176   137.20202445983887   \n",
      "\n",
      "\n",
      "right     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 152.93216705322266\n",
      "left：23.609399795532227   91.87602996826172   \n",
      "right：23.658037185668945   153.05376052856445   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 1198.1983184814453\n",
      "left：1197.7646350860596   934.7498416900635   \n",
      "right：1197.934865951538   163.3162498474121   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 639.6546363830566\n",
      "left：4.393577575683594   1198.2429027557373   \n",
      "right：389.1639709472656   135.7712745666504   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 1198.7333297729492\n",
      "left：1197.7686882019043   1197.5822448730469   \n",
      "right：1197.8294849395752   292.7403450012207   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 134.49859619140625\n",
      "left：1198.1699466705322   1197.97945022583   \n",
      "right：385.6985569000244   158.01072120666504   \n",
      "\n",
      "\n",
      "up     speed: 50     time: 0\n",
      "\n",
      "\n",
      "\n",
      "front 61.52629852294922\n",
      "left：16.39080047607422   8.138656616210938   \n",
      "right：114.49646949768066   75.52576065063477   \n",
      "\n",
      "\n",
      "right     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 7.368564605712891\n",
      "left：71.24972343444824   71.94685935974121   \n",
      "right：8.118391036987305   67.89374351501465   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 73.77886772155762\n",
      "left：254.730224609375   61.29932403564453   \n",
      "right：8.102178573608398   10.48135757446289   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 41.62144660949707\n",
      "left：62.89219856262207   50.566673278808594   \n",
      "right：35.367488861083984   9.690999984741211   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 15.191078186035156\n",
      "left：17.477035522460938   74.17607307434082   \n",
      "right：15.677452087402344   15.912532806396484   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n",
      "\n",
      "\n",
      "\n",
      "front 380.51867485046387\n",
      "left：68.21393966674805   70.47152519226074   \n",
      "right：15.401840209960938   14.327764511108398   \n",
      "\n",
      "\n",
      "left     speed: 50     time: 0.3\n"
     ]
    }
   ],
   "source": [
    "#!/usr/bin/python  \n",
    "# coding=utf-8  \n",
    "#本段代码实现树莓派智能小车的红外避障效果\n",
    "#代码使用的树莓派GPIO是用的BOARD编码方式。 \n",
    "import RPi.GPIO as GPIO\n",
    "#电机控制器\n",
    "from motor import Motor\n",
    "#红外距离传感器\n",
    "from infrared_sensor import InfraredSensor\n",
    "#超声波避障\n",
    "from adafruit.Adafruit import Adafruit\n",
    "\n",
    "import sys\n",
    "\n",
    "#动作常量\n",
    "ACTION_UP = 0\n",
    "ACTION_DOWN = 1\n",
    "ACTION_LEFT = 2\n",
    "ACTION_RIGHT = 3\n",
    "\n",
    "#转向常量\n",
    "DIR_NONE = 0\n",
    "DIR_LEFT = 1\n",
    "DIR_RIGHT =2\n",
    "DIR_UP = 3\n",
    "DIR_DOWN = 4\n",
    "\n",
    "#碰撞距离\n",
    "COLL_DIST = 35\n",
    "#左右两碰撞距离\n",
    "LEFT_RIGHT_COLL_DIST = [16,16]\n",
    "\n",
    "#红外距离传感器\n",
    "infrared_sensor = InfraredSensor()\n",
    "#电机控制类\n",
    "motor = Motor()\n",
    "#超声波\n",
    "adafruit = Adafruit()\n",
    "            \n",
    "def setup():\n",
    "    GPIO.setwarnings(False)\n",
    "    GPIO.setmode(GPIO.BCM)       # Numbers GPIOs by physical location\n",
    "\n",
    "    #红外距离传感器初始化\n",
    "    infrared_sensor.init()\n",
    "    #初始化电机\n",
    "    motor.init()\n",
    "    #初始化超声波\n",
    "    adafruit.init()\n",
    "    \n",
    "#判断距离是否短\n",
    "def is_short(dist,coll_dist=COLL_DIST):\n",
    "    return (dist < coll_dist)\n",
    "\n",
    "#有一个距离都短都返回true\n",
    "def is_has_short(dists,coll_dist=LEFT_RIGHT_COLL_DIST,dist_num=0):\n",
    "    num=len(dists)\n",
    "    #根据参数要求判断前面多少个变量\n",
    "    if dist_num>0:\n",
    "        num=dist_num\n",
    "    \n",
    "    for i in range(num):\n",
    "        if is_short(dists[i],coll_dist[i]):\n",
    "            return True\n",
    "    return False\n",
    "\n",
    "#判断距离是否远\n",
    "def is_far(dist,coll_dist=COLL_DIST):\n",
    "    return (dist >= coll_dist)\n",
    "\n",
    "#判断所有距离都远\n",
    "def is_all_far(dists,coll_dist=LEFT_RIGHT_COLL_DIST):\n",
    "    for i in range(len(dists)):\n",
    "        if is_far(dists[i],coll_dist[i])==False:\n",
    "            return False\n",
    "    return True\n",
    "\n",
    "#比较两个距离数组的距离，如果dist1短则返回true\n",
    "def compare_dist_short(dists1,dists2):\n",
    "    sum_dists1 = 0\n",
    "    sum_dists2 = 0\n",
    "    for i in range(len(dists1)):\n",
    "        sum_dists1 += dists1[i]\n",
    "        sum_dists2 += dists2[i]\n",
    "    return (sum_dists1 < sum_dists2)\n",
    "\n",
    "def left_dists(adafruit,step_angle=0.125,len=4):\n",
    "    dists = []\n",
    "    for i in range(len):\n",
    "        dists.append(adafruit.left_rate(step_angle*(i+1)))\n",
    "    return dists\n",
    "\n",
    "def right_dists(adafruit,step_angle=0.125,len=4):\n",
    "    dists = []\n",
    "    for i in range(len):\n",
    "        dists.append(adafruit.right_rate(step_angle*(i+1)))\n",
    "    return dists\n",
    "\n",
    "def print_dists(label,dists):\n",
    "    val = label+\"：\"\n",
    "    for i in range(len(dists)):\n",
    "        val += str(dists[i])+\"   \"\n",
    "    print(val)\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    setup()\n",
    "\n",
    "    #超声波\n",
    "    adafruit.start()\n",
    "    #启动红外传感器\n",
    "    infrared_sensor.start()\n",
    "    #启动电机\n",
    "    motor.start()\n",
    "\n",
    "    #是否有阻碍物\n",
    "    is_has_hinder = False\n",
    "    \n",
    "    #记录上次是转左还是右，0为空，1为左，2为右\n",
    "    # DIR_NONE = 0\n",
    "    # DIR_LEFT = 1\n",
    "    # DIR_RIGHT =2\n",
    "    last_is_left_or_right = DIR_NONE\n",
    "    last_is_up_or_down = DIR_NONE\n",
    "\n",
    "    #动作\n",
    "    action = ACTION_UP\n",
    "    action_speed = 50\n",
    "    action_time = 0.2\n",
    "\n",
    "    try:\n",
    "        while True:\n",
    "            dist_front = 0\n",
    "            dist_left = []\n",
    "            dist_right = []\n",
    "\n",
    "            dist_front = adafruit.front()#获取超声波的陀机角度\n",
    "            #红外距离\n",
    "            SR_2 = infrared_sensor.getRight()\n",
    "            SL_2 = infrared_sensor.getLeft()\n",
    "\n",
    "            #超声波的陀机转向左右前方判断距离\n",
    "            dist_left = left_dists(adafruit,0.45,len(LEFT_RIGHT_COLL_DIST))\n",
    "            dist_right = right_dists(adafruit,0.45,len(LEFT_RIGHT_COLL_DIST))\n",
    "            \n",
    "            print(\"\\n\\n\\nfront\",dist_front)\n",
    "            print_dists(\"left\",dist_left)\n",
    "            print_dists(\"right\",dist_right)\n",
    "            print(\"\\n\")\n",
    "            \n",
    "            #左右两边是否有短距离\n",
    "            is_has_short_left = is_has_short(dist_left,LEFT_RIGHT_COLL_DIST)\n",
    "            is_has_short_right = is_has_short(dist_right,LEFT_RIGHT_COLL_DIST)\n",
    "\n",
    "            #如果上次没有障碍物并且现在前面有障碍物\n",
    "            if is_has_hinder==False and is_short(dist_front):\n",
    "                action = ACTION_DOWN #向后\n",
    "                last_is_up_or_down = DIR_DOWN\n",
    "                action_speed = 50\n",
    "                action_time = 0.3\n",
    "                is_has_hinder = True #标记有障碍物\n",
    "\n",
    "            #如果上次是后退的或者左右两边有障碍物\n",
    "            elif last_is_up_or_down==DIR_DOWN or (SR_2 ==False or SL_2 ==False) or (is_has_short_left or is_has_short_right):\n",
    "                is_has_hinder = True #标记有障碍物\n",
    "                \n",
    "                #是往左还是右，False是左，True是右\n",
    "                is_left_or_right = False\n",
    "\n",
    "                #如果左边的距离比右边的远，则往左边转动\n",
    "                if last_is_up_or_down==DIR_DOWN and last_is_left_or_right == DIR_LEFT:\n",
    "                    is_left_or_right = False\n",
    "                elif last_is_up_or_down==DIR_DOWN and last_is_left_or_right == DIR_RIGHT:\n",
    "                    is_left_or_right = True\n",
    "                elif SR_2 ==False or compare_dist_short(dist_left,dist_right)==False:\n",
    "                    is_left_or_right = False\n",
    "                    last_is_up_or_down = DIR_NONE#清空最后一次前进或者的状态\n",
    "                else:\n",
    "                    is_left_or_right = True\n",
    "                    last_is_up_or_down = DIR_NONE#清空最后一次前进或者的状态\n",
    "                    \n",
    "                if is_left_or_right==False:\n",
    "                    action = ACTION_LEFT #向左\n",
    "                    last_is_left_or_right = DIR_LEFT#最后一次转向\n",
    "                    action_speed = 50\n",
    "                    action_time = 0.3\n",
    "                else:\n",
    "                    action = ACTION_RIGHT #向右\n",
    "                    last_is_left_or_right = DIR_RIGHT#最后一次转向\n",
    "                    action_speed = 50\n",
    "                    action_time = 0.3\n",
    "            else:\n",
    "                is_has_hinder = False #标记没有有障碍物\n",
    "                action = ACTION_UP #向前\n",
    "                action_speed = 50\n",
    "                action_time = 0\n",
    "\n",
    "            #如果没有障碍物则重置相关变量\n",
    "            if is_has_hinder == False:\n",
    "                #最后一次转向是左还是右\n",
    "                last_is_left_or_right = DIR_NONE\n",
    "                last_is_up_or_down = DIR_NONE\n",
    "            \n",
    "            # 执行动作\n",
    "            if action == ACTION_UP:\n",
    "                print (\"up\",\"    speed:\",action_speed,\"    time:\",action_time)\n",
    "                motor.up(action_speed,action_time)\n",
    "            elif action == ACTION_DOWN:\n",
    "                print (\"down\",\"    speed:\",action_speed,\"    time:\",action_time)\n",
    "                motor.down(action_speed,action_time)\n",
    "                motor.stop(0.5)\n",
    "            elif action == ACTION_LEFT:\n",
    "                print (\"left\",\"    speed:\",action_speed,\"    time:\",action_time)\n",
    "                motor.left(action_speed,action_time)\n",
    "                motor.stop(0.5)\n",
    "            elif action == ACTION_RIGHT:\n",
    "                print (\"right\",\"    speed:\",action_speed,\"    time:\",action_time)\n",
    "                motor.right(action_speed,action_time)\n",
    "                motor.stop(0.5)\n",
    "            \n",
    "    except KeyboardInterrupt:  # When 'Ctrl+C' is pressed, the child program destroy() will be  executed.\n",
    "        GPIO.cleanup()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f0eedd64-255f-4898-ae1a-a4b59f109134",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6628f5df-211d-4b61-8978-5e93b294d0f4",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "tensorflow_env",
   "language": "python",
   "name": "tensorflow_env"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
